#include "BallConstraint.h"
#include "MultibodySystem.h"

using namespace RSIM;

///////////////////////////////////////////////////////

void BallConstraint::calcPositionLevelError(){
        const int& plen = this->Parent_Path_.getLength();
        const int& clen = this->Child_Path_.getLength();
        
        double *pos_err = this->Pos_Error_.wPtr();
        const double *p_beta = this->Parent_cBetak_[plen-1].wPtr();        
        
        if(clen){
                // This is the case when child is not ground
                const double *c_beta = Child_cBetak_[clen-1].wPtr();                                
                pos_err[0] = p_beta[0] - c_beta[0];
                pos_err[1] = p_beta[1] - c_beta[1];
                pos_err[2] = p_beta[2] - c_beta[2];
        }
        else{
                const double *c_beta = this->Child_Bod_FM_.P().Ptr();                
                pos_err[0] = p_beta[0] - c_beta[0];
                pos_err[1] = p_beta[1] - c_beta[1];
                pos_err[2] = p_beta[2] - c_beta[2];
        }        
}

///////////////////////////////////////////////////////

void BallConstraint::calcConstraintJacobian(const int& ctoumap,Matrix *CJac)const{        
        double dv[3];
        // Sweep for parent branch
        for(int i=0;i<Parent_Path_.getLength()-1;++i){
                const RigidBodyNode *Nd = pMBS_->getRigidBodyNode( Parent_Path_[i] );
                const int& dof = Nd->getDOF();
                                
                for(int j=0;j<dof;++j){                        
                        Nd->data_->calcShiftPartialLinear(Parent_cBetak_[i],j,dv);
                        (*CJac)(ctoumap,this->pumap_[i]+j) = dv[0];
                        (*CJac)(ctoumap+1,this->pumap_[i]+j) = dv[1];
                        (*CJac)(ctoumap+2,this->pumap_[i]+j) = dv[2];
                }
        }
        
        // Sweep for Child branch
        for(int i=0;i<Child_Path_.getLength()-1;++i){
                const RigidBodyNode *Nd = cMBS_->getRigidBodyNode( Child_Path_[i] );
                const int& dof = Nd->getDOF();
                
                for(int j=0;j<dof;++j){                        
                        Nd->data_->calcShiftPartialLinear(Child_cBetak_[i],j,dv);
                        (*CJac)(ctoumap,this->cumap_[i]+j) = -dv[0];
                        (*CJac)(ctoumap+1,this->cumap_[i]+j) = -dv[1];
                        (*CJac)(ctoumap+2,this->cumap_[i]+j) = -dv[2];
                }
        }        
        
}

///////////////////////////////////////////////////////